import os
import time
import threading
import rclpy
import cv2
from rclpy.node import Node
import face_recognition
from ament_index_python.packages import get_package_share_directory
from chapt4_interfaces.srv import FaceDetector  # 导入服务接口
from cv_bridge import CvBridge
from rcl_interfaces.srv import SetParameters
from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType

class FaceDetectClientNode(Node):
    def __init__(self):
        super().__init__('face_detect_client_node')
        self.bridge = CvBridge()  # 创建cv_bridge实例
        self.default_path = os.path.join(get_package_share_directory('demo_python_service'), 'resource/face1.jpg')
        self.get_logger().info('人脸检测客户端启动............................')
        self.client = self.create_client(FaceDetector, 'face_detect')
        
        self.image = cv2.imread(self.default_path)

    def call_set_parameters(self, parameters):
        """
        调用服务设置服务端参数
        """
        # 1 创建客户端
        update_param = self.create_client(SetParameters, '/face_detect_node/set_parameters')
        while update_param.wait_for_service(timeout_sec=1.0) is False:
            self.get_logger().info('service not available, waiting again...')
        # 2 创建请求
        request = SetParameters.Request()
        request.parameters = parameters
        # 3 发送请求
        future = update_param.call_async(request)
        rclpy.spin_until_future_complete(self, future)  # 等候等待服务可用
        response = future.result()
        return response

    def update_detect_model(self, model):
        """
        根据传入的model更新Parameters,然后调用call_set_paramerters函数更新服务端参数
        """
        param = Parameter()
        param.name = 'model'
        # 创建特定的参数格式
        param_value = ParameterValue()
        param_value.string_value = model
        param.value.type = ParameterType.PARAMETER_STRING
        param.value = param_value
        # 请求更新参数
        response = self.call_set_parameters([param])
        for result in response.results:
            if result.successful:
                self.get_logger().info('successfully set parameter: ' + result.reason)
            else:
                self.get_logger().error('failed to set parameter: ' + result.reason)

    def send_request(self):
        # 1 判断服务是否可用
        while self.client.wait_for_service(timeout_sec=1.0) is False:
            self.get_logger().info('正在等待服务启动...')
        # 2 创建请求
        request = FaceDetector.Request()
        request.image = self.bridge.cv2_to_imgmsg(self.image)  # 将cv2格式的图像转换为ros2的图像消息
        # 3 发送请求
        future = self.client.call_async(request)  # 异步调用服务 此刻的future还没有得到响应结果，需要等待服务
        self.get_logger().info('正在调用服务...')
        rclpy.spin_until_future_complete(self, future)

        # 4 处理响应结果
        response = future.result()
        self.get_logger().info(f'人脸检测结束，人脸个数为：{response.number}，耗时{response.use_time}s')
        self.show_result(response)

    def show_result(self, response):
        for i in range(response.number):
            # 获取人脸位置
            top = response.top[i]
            left = response.left[i]
            right = response.right[i]
            bottom = response.bottom[i]
            cv2.rectangle(self.image, (left, top), (right, bottom), (255, 0, 0), 4)  # 绘制人脸框
        cv2.imshow('face', self.image)
        cv2.waitKey(0)  # 等待用户按键

def main():
    rclpy.init()
    node = FaceDetectClientNode()
    node.update_detect_model('cnn')
    node.send_request()
    # node.update_detect_model('hog')
    # node.send_request()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()